import cmath
import math

import numpy
from numpy import ndarray

from services.automatic_control.schemas.configs import FFTSpectrum, ComplexWaveform


class MAPro:
    __slots__ = []

    @staticmethod
    def two_sided_fft_spectrum(signal_V, dt) -> tuple[ndarray, float, float]:
        fft_x = numpy.fft.fft(signal_V)
        n = math.floor(len(signal_V) / 2)

        fft_x = [x * (1 / len(signal_V)) for x in fft_x[:]]
        fft_x = numpy.roll(fft_x, int(len(fft_x) / 2))
        df = 1 / (dt * len(signal_V))
        f0 = df * (-n)

        return fft_x, df, f0

    @staticmethod
    def convert_to_db(spectrum) -> list:
        db_spectrum = [(float('-inf') if x == 0 else math.log10(x)) * 20 for x in spectrum]
        return db_spectrum

    @staticmethod
    def fft_spectrum_mag_phase(waveform: ComplexWaveform) -> FFTSpectrum:
        # [x * y for x, y in zip(waveform.Y, numpy.hanning(len(waveform.Y)))]
        windowed_x = numpy.multiply(waveform.data, numpy.hanning(len(waveform.data)))
        waveform.data = windowed_x

        fft_x, df, f0 = MAPro.two_sided_fft_spectrum(waveform.data, waveform.dt)
        r_ar = []
        e_ar = []

        for z in fft_x:
            r, theta = cmath.polar(z)
            r_ar.append(r)
            e_ar.append(theta)

        mag_spectrum = MAPro.convert_to_db(r_ar)
        return FFTSpectrum(f0=f0, df=df, data=mag_spectrum)

    @staticmethod
    def extract_multiple_tone_information(time_signal_in: ComplexWaveform, threshold, max_num_tones,
                                          output_sorting) -> list:
        spectrum, spectrum_length = MAPro.fft_with_hanning(time_signal_in.data)
        fs, f0, df, spectral_info = MAPro.get_waveform_info(time_signal_in)

        fft_spectrum = FFTSpectrum(f0=f0, df=df, data=spectrum.tolist())

        criterion = {'single/multiple': 'Single Max Peak',
                     'threshold': threshold}

        three_fingers, _ = MAPro.locate_three_fingers(fft_spectrum, spectral_info, criterion, True)

        t1 = []
        for tf in three_fingers:
            t1.append(MAPro.tone_estimation_fft_input_cdb(tf, spectral_info))

        t2 = []

        for i, tone_info in enumerate(t1[:]):
            freq = t1[i][0] * fs
            amp = t1[i][1] * math.sqrt(2)
            phase = t1[i][2] * 1
            t2.append((freq, amp, phase))

        t3 = []
        for tone_info in t2:
            if tone_info[1] > threshold:
                t3.append(tone_info)

        t3 = MAPro.sort_tone_info(t3)[::-1]
        t3 = t3[:max_num_tones]

        if output_sorting != 'increasing frequency':
            t3 = sorted(t3, key=lambda tone: tone[0])

        return t3

    @staticmethod
    def sort_tone_info(tone_info_ar) -> list:
        sorted_by_amplitude = sorted(tone_info_ar, key=lambda tone: tone[1])
        return sorted_by_amplitude

    @staticmethod
    def get_waveform_info(waveform) -> tuple[float, float, float, dict]:
        f0 = -1 * (math.floor(len(waveform.data) / 2) * (1 / (waveform.dt * len(waveform.data))))
        fs = 1 / waveform.dt
        df = 1 / (len(waveform.data) * waveform.dt)

        spectral_info = {'spectral_type': 'fft_spectrum[complex]', 'linear/db': 'linear', 'window': 'hanning',
                         'window_size': len(waveform.data), 'fft_size': len(waveform.data)}

        return fs, f0, df, spectral_info

    @staticmethod
    def fft_with_hanning(time_signal_data) -> tuple[numpy, int]:
        fft_size = len(time_signal_data)

        X = [(x * math.sqrt(2)) / fft_size for x in time_signal_data]

        windowed_X = [x * y for x, y in zip(X, numpy.hanning(fft_size))]

        # complex_hann_spectrum = numpy.fft.fftshift(windowed_X)
        complex_hann_spectrum = numpy.fft.fftshift(numpy.fft.fft(windowed_X))

        return complex_hann_spectrum, fft_size

    @staticmethod
    def locate_three_fingers(fft_spectrum: FFTSpectrum, spectral_info, criterion, includes_endpoints) -> tuple[
            list, int]:
        three_fingers = []
        if len(fft_spectrum.data) - (1 if includes_endpoints else 0) > 2:
            s = criterion.get('single/multiple') == 'Single Max Peak' and criterion.get('threshold') <= 0

            t1 = [abs(x) for x in fft_spectrum.data[:len(fft_spectrum.data) - (1 if includes_endpoints else 0)]]
            t2 = [x for x in fft_spectrum.data[:len(fft_spectrum.data) - (1 if includes_endpoints else 0)]]

            max_t = (max(t1) * 0.10 if s else criterion.get('threshold')) / 1.6

            t1.append(0.00)

            cont = True
            index = 0
            while cont:
                a = t1[index]
                b = t1[index + 1]
                c = t1[index + 2]
                if max_t > a and max_t > b and max_t > c:
                    pass
                else:
                    if (a >= b and a > c and index == 0) or (b >= a and b >= c):
                        new_fft_spectrum = FFTSpectrum(f0=0.0, df=0.0, data=[])
                        fft_size = spectral_info.get('fft_size')
                        df = fft_spectrum.df
                        f0 = fft_spectrum.f0

                        new_fft_spectrum.f0 = (1 / (fft_size * df) * df) * index + 1 / (fft_size * df) * f0

                        new_fft_spectrum.df = (1 / (
                                spectral_info.get('fft_size') * fft_spectrum.df) * fft_spectrum.df)

                        new_fft_spectrum.data = t2[index: index + 3]

                        three_fingers.append(new_fft_spectrum)
                        index += 1

                    else:
                        pass
                index += 1
                cont = index < len(fft_spectrum.data) - (1 if includes_endpoints else 0) - 1

            pass
        else:
            pass
        return three_fingers, len(three_fingers)

    @staticmethod
    def tone_estimation_fft_input_cdb(three_fingers: FFTSpectrum, spectral_info) -> tuple[float, float, float]:
        initial_guess, range = MAPro.initial_guess_for_tone_estimation_fft_input(three_fingers, spectral_info)

        f0 = three_fingers.f0
        df = three_fingers.df * 2 + f0

        window = spectral_info.get('window')
        N = spectral_info.get('window_size')
        cont = True
        index = 0
        amplitude = 0
        phase = 0
        while cont:
            amplitude, phase, gf = MAPro.tone_estimation_subroutine_gf_fft_input_cdb(three_fingers, N, window,
                                                                                     frequency=initial_guess)
            gdf = MAPro.tone_estimation_subroutine_gf_first_derivative_fft_input_cdb(three_fingers, window, N,
                                                                                     frequency=initial_guess)

            initial_guess = initial_guess - (gf / gdf)

            x1 = not (df >= initial_guess >= f0)
            x2 = abs(gf / gdf) <= 1.00e-8
            x3 = index >= 100

            cont = not (x1 or x2 or x3)
            index += 1

        return initial_guess, amplitude, phase

    @staticmethod
    def initial_guess_for_tone_estimation_fft_input(three_fingers: FFTSpectrum, spectral_info):
        initial_guess, range = MAPro.initial_guess_for_tone_estimation_amplitude_input(
            FFTSpectrum(f0=three_fingers.f0, df=three_fingers.df, data=[abs(x) for x in three_fingers.data]),
            spectral_info)

        return initial_guess, range

    @staticmethod
    def initial_guess_for_tone_estimation_amplitude_input(three_fingers: FFTSpectrum, spectral_info) -> tuple[
          float, tuple]:

        gdf1 = MAPro.tone_estimation_subroutine_first_derivative(three_fingers,
                                                                 spectral_info.get('window'),
                                                                 spectral_info.get('window_size'),
                                                                 frequency=three_fingers.f0 + three_fingers.df * 1.00)

        gdf2 = MAPro.tone_estimation_subroutine_first_derivative(three_fingers,
                                                                 spectral_info.get('window'),
                                                                 spectral_info.get('window_size'),
                                                                 frequency=three_fingers.f0 + three_fingers.df * 0.75)

        f1 = three_fingers.f0 + three_fingers.df * 0.75 if gdf1 * gdf2 >= 0 else three_fingers.f0 + three_fingers.df * 1.00
        f2 = three_fingers.f0 + three_fingers.df * 1.00 if gdf1 * gdf2 >= 0 else three_fingers.f0 + three_fingers.df * 1.25

        _, gf1 = MAPro.tone_estimation_subroutine_gf_amplitude_input(three_fingers, f1, spectral_info.get('window'),
                                                                     spectral_info.get('window_size'))

        _, gf2 = MAPro.tone_estimation_subroutine_gf_amplitude_input(three_fingers, f2, spectral_info.get('window'),
                                                                     spectral_info.get('window_size'))

        x = ((gf1 / (gf1 - gf2)) * (f2 - f1)) + f1

        coherent_gain, enbw, _3db_bw, _6db_bw = MAPro.window_parameters(spectral_info.get('window'))

        m = (max(spectral_info.get('fft_size') / spectral_info.get('window_size') * _6db_bw,
                 2.0) / 2.0) * three_fingers.df
        upper_limit = three_fingers.df + three_fingers.f0 + m
        lower_limit = three_fingers.df + three_fingers.f0 - m

        fromto = (upper_limit, lower_limit)

        initial_guess = x if lower_limit < x < upper_limit else (upper_limit + lower_limit) / 2.0
        return initial_guess, fromto

    @staticmethod
    def window_frequency_response(window, f, N) -> float:
        if window == 'hanning':
            coefs = [0.5, 0.5]
        else:
            raise NotImplemented

        res = 0 + 0j
        sign = 1.0
        for index, c in enumerate(coefs):
            f1 = MAPro.rect_window_frequency_response(f - index / N, N)
            f2 = MAPro.rect_window_frequency_response(f + index / N, N)

            fsum = f1 + f2
            new_result = c * fsum * sign
            res = res + new_result
            sign *= -1

        Hf = res / (coefs[0] * 2.0)
        return Hf

    @staticmethod
    def rect_window_frequency_response(f, N) -> complex:

        r = numpy.sinc(f * N) / numpy.sinc(f)
        theta = (f * numpy.pi) * (1.00 - N)

        Hf = complex(r * math.cos(theta), r * math.sin(theta))

        return Hf

    @staticmethod
    def window_frequency_response_array(f_ar, N, window) -> list:
        hf = []
        for f in f_ar:
            hf.append(MAPro.window_frequency_response(window, f, N))

        return hf

    @staticmethod
    def window_parameters(window) -> tuple[float, float, float, float]:
        if window == 'hanning':
            enbw = 1.5
            coherent_gain = 0.5
            _6db_bw = 2.0
            _3db_bw = 1.44
            return coherent_gain, enbw, _3db_bw, _6db_bw
        raise NotImplemented

    @staticmethod
    def tone_estimation_subroutine_gf_amplitude_input(three_fingers: FFTSpectrum, frequency, window, N) -> tuple[
        float, float]:
        f_normalized = []
        for i in range(3):
            f_normalized.append((three_fingers.f0 + three_fingers.df * i) - frequency)

        f = [abs(x) for x in MAPro.window_frequency_response_array(f_normalized, N, window)]
        a1 = []
        a2 = []

        for i in range(2):
            a1.append(sum(three_fingers.data[: len(three_fingers.data) - i]))
            a2.append(sum(f[: len(three_fingers.data) - i]))

        amplitude = a1[0] / a2[0]
        gf = abs(a1[0] * a2[1]) - abs(a1[1] * a2[0])

        return amplitude, gf

    @staticmethod
    def tone_estimation_subroutine_first_derivative(three_fingers: FFTSpectrum, window, N, frequency) -> float:
        ampl_x, x = MAPro.tone_estimation_subroutine_gf_amplitude_input(three_fingers, frequency + 2 ** (-30), window,
                                                                        N)
        ampl_y, y = MAPro.tone_estimation_subroutine_gf_amplitude_input(three_fingers, frequency - 2 ** (-30), window,
                                                                        N)

        gdf = (x - y) / (2 ** (-30) * 2)
        return gdf

    @staticmethod
    def tone_estimation_subroutine_gf_fft_input_cdb(three_fingers: FFTSpectrum, N, window, frequency) -> tuple[
        float, float, float]:

        f_normalized = []
        for i in range(3):
            f_normalized.append((three_fingers.f0 + three_fingers.df * i) - frequency)

        f = MAPro.window_frequency_response_array(f_normalized, N, window)

        p = abs(three_fingers.f0 - frequency)
        m = abs(frequency - (2 * three_fingers.df + three_fingers.f0))
        n = three_fingers.df * 1.00e-6

        s = p <= n or m <= n

        spectrum_length = len(three_fingers.data)
        da1 = []
        da2 = []
        for i in range(2):
            a = three_fingers.data[:spectrum_length - i]
            b = f[:spectrum_length - i]
            dot1 = numpy.vdot(a, b)
            dot2 = numpy.vdot(b, b)
            da1.append(dot1)
            da2.append(dot2)

        gf = abs(da1[0] * da2[1]) - abs(da1[1] * da2[0])  # abs(x1 * y2) - abs(x2 * y1)

        if s:
            da1[0] = da1[0] / 2.0

        amplitude, theta = cmath.polar(da1[0] / da2[0])

        phase = theta * (1 / math.pi) * 180.0

        return amplitude, -phase, gf

    @staticmethod
    def tone_estimation_subroutine_gf_first_derivative_fft_input_cdb(three_fingers: FFTSpectrum, window, N,
                                                                     frequency) -> float:
        amplitude1, phase1, gf1 = MAPro.tone_estimation_subroutine_gf_fft_input_cdb(three_fingers, N, window,
                                                                                    frequency + 2 ** (-30))
        amplitude2, phase2, gf2 = MAPro.tone_estimation_subroutine_gf_fft_input_cdb(three_fingers, N, window,
                                                                                    frequency - 2 ** (-30))
        gdf = (gf1 - gf2) / (2 * 2 ** (-30))
        return gdf
